#include "C3DConverter.h"

#include <MMM/Motion/Plugin/MoCapMarkerPlugin/MoCapMarkerSensor.h>
#include <MMM/Motion/Plugin/MoCapMarkerPlugin/MoCapMarkerSensorMeasurement.h>

#include <MMM/Exceptions.h>

using namespace MMM;

C3DConverter::C3DConverter(const std::string &motionFilePath) : motionFilePath(motionFilePath)
{
    MMM::MotionReaderC3DPtr motionReaderC3D(new MMM::MotionReaderC3D());
    prefixMarkerData = motionReaderC3D->loadC3D(motionFilePath);
}

MotionList C3DConverter::convertMotions(const std::vector<std::tuple<std::string, std::string> > &motionNameToMarkerPrefix, const std::string &sensorName, const std::string &sensorDescription) {
    MotionList motions;
    for (const auto &mapping : motionNameToMarkerPrefix) {
        if (!prefixMarkerData->hasMarkerData(std::get<1>(mapping))) {
            std::string error = "No motion capture markers with markerprefix '" + std::get<1>(mapping) + "' found! Found the following prefixes: ";
            for (const auto &prefix : prefixMarkerData->getPrefixes()) {
                error += prefix + " ";
            }
            throw MMM::Exception::MMMException(error);
        }

        std::map<float, MarkerDataPtr> markerDataMap = prefixMarkerData->getMarkerData(std::get<1>(mapping));

        // create MoCapMarker sensor
        MoCapMarkerSensorPtr moCapMarkerSensor(new MoCapMarkerSensor());
        moCapMarkerSensor->setName(sensorName);
        moCapMarkerSensor->setDescription(sensorDescription);
        for (const auto &m : markerDataMap) {
            moCapMarkerSensor->addSensorMeasurement(MoCapMarkerSensorMeasurementPtr(new MoCapMarkerSensorMeasurement(m.first, m.second)));
        }

        // create motion
        MotionPtr motion(new Motion(std::get<0>(mapping), nullptr, nullptr, nullptr, motionFilePath));
        motion->addSensor(moCapMarkerSensor); // No Exception handling, because the exception should not occure!

        motions.push_back(motion);
    }
    return motions;
}

MMM::PrefixMarkerDataPtr C3DConverter::getPrefixMarkerData() {
    return prefixMarkerData;
}
